/*
 * MoCrap Bone Test I
 */

import java.util.*;

import java.nio.FloatBuffer;

import org.lwjgl.BufferUtils;
import org.lwjgl.LWJGLException;
import org.lwjgl.opengl.ARBTransposeMatrix;
import org.lwjgl.opengl.Display;
import org.lwjgl.opengl.DisplayMode;
import org.lwjgl.opengl.GL11;
import org.lwjgl.opengl.GLContext;
import org.lwjgl.util.glu.GLU;

//Draws a skeleton

public class MoCrapTest {

    public ASFParser asf = null;
    public AMCParser amc = null;
    public ArrayList<Motion> motions = null;
    public int moCapFrames = 0;
    public int curFrame = 1;

	public static void main(String[] args) {
		new MoCrapTest().execute();
		System.exit(0);
	}

	private void execute() {
		try {
            parse();
            init();
		} catch (LWJGLException le) {
			le.printStackTrace();
			System.out.println("Failed to initialize MoCrap.");
			return;
		}

		loop();

		destroy();
	}

    private void parse()
    {
        asf = new ASFParser();
        asf.parseFile("07.asf");

        amc = new AMCParser();
        motions = amc.parseFile("07_04.amc");

        moCapFrames = amc.getMotion("root", motions).size();

        System.out.println("Motion capture frame count : "+moCapFrames);
    }

	private void destroy() {
		Display.destroy();
	}

	private void loop() {

		while (!Display.isCloseRequested()) {

			clearScreen();

          //our drawing
            reset();
            movePen(1.5f,0.0f,-90.0f);

            curFrame++;
            if (curFrame > moCapFrames)
                curFrame = 1;

        //    drawMobileSkeleton(asf.root,curFrame);
            drawStaticSkeleton(asf.root);

			Display.update();
		}
	}

    private void drawStaticSkeleton(Bone root)
    {
        if (root != null)
        {
            drawBone(root);
            if (root.children != null)
            {
                for (int i = 0; i < root.children.size(); i++)
                {
                    drawStaticSkeleton(root.children.get(i));
                }
            }
        }
    }

/*
    private void drawStaticSkeleton(Bone root)
    {
        if (root != null)
        {
            Rotation rot = root.getRotation();

            Vector3D pos = root.getRelPosition();

            pushMat();
            //movePen(pos.x,pos.y,pos.z);

            if (!rot.axis.isZero())
            {
                rotatePen(rot.angle,rot.axis.x,rot.axis.y,rot.axis.z);
            }

            movePen(0,root.getLength(),0);

            penBone(root.getLength());

        
        
            if (root.children != null)
            {
                for (int i = 0; i < root.children.size(); i++)
                {
                    drawStaticSkeleton(root.children.get(i));
                }
            }
                        popMat();
        }
    }
*/

    private void drawMobileSkeleton(Bone root, int frame)
    {
        if (root != null)
        {
            pushMat();
            drawBoneAtFrame(root,frame);
            if (root.children != null)
            {
                for (int i = 0; i < root.children.size(); i++)
                {
                    drawMobileSkeleton(root.children.get(i),frame);
                }
            }
            popMat();
        }
    }

    //Relative movement of drawing "pen"
    private void movePen(float x, float y, float z)
    {
        GL11.glTranslatef(x,y,z); //translate the "pen"
    }

    //Rotate deg degrees about vector (vx,vy,vz)
    private void rotatePen(float deg, float vx, float vy, float vz)
    {
        GL11.glRotatef(deg,vx,vy,vz); //rotate pen
    }

    private void pushMat()
    {
        GL11.glPushMatrix();
    }

    private void popMat()
    {
        GL11.glPopMatrix();
    }

    private void clearScreen()
    {
        GL11.glClear(GL11.GL_COLOR_BUFFER_BIT | GL11.GL_DEPTH_BUFFER_BIT);
    }

    private void reset()
    {
        GL11.glLoadIdentity(); //reset view
    }

    private void drawBoneAtFrame(Bone b, int frame)
    {
        double[] rotations = amc.getMotionFrame(b.getName(),motions,frame-1);
        Rotation rot;

        rot = b.getRotation();

        Vector3D ax = Vector3D.zero();

        if (b.getAxis() != null)
        {
            ax = new Vector3D((float)b.getAxis()[0],
                (float)b.getAxis()[1],(float)b.getAxis()[2]);
        }

        if (rotations != null)
        {
            drawBone((float)b.getLength(), b.getRelPosition(), ax, rotations,
                rot);
        }
        else
        {
            double[] zeroRs = new double[3];
            zeroRs[0] = 0.0; zeroRs[1] = 0.0; zeroRs[2] = 0.0;
            drawBone((float)b.getLength(), b.getRelPosition(), ax, zeroRs,
                rot);
        }
    }

    private void drawBone(Bone b)
    {
        Rotation rot = b.getRotation();
        drawBone((float)b.getLength(), b.getAbsPosition(), rot.axis, rot.angle);
    }

    private void drawBone(float length, Vector3D axis, float angle)
    {
        drawBone(length, Vector3D.zero(), axis, angle);
    }

    private void drawBone(float length, Vector3D pos, Vector3D axis, float angle)
    {
        pushMat();
        movePen(pos.x,pos.y,pos.z);

        if (!axis.isZero())
        {
            rotatePen(angle,axis.x,axis.y,axis.z);
        }

        penBone(length);

        popMat();
    }

    private void drawBone(float length, Vector3D pos, Vector3D axis, 
        double[] rots, Rotation staticRot)
    {
        double[] rotsStd = new double[3];

        for (int i = 0; i < 3; i++)
        {
            if (i < rots.length)
            {
                rotsStd[i] = rots[i];
            }
            else
            {
                rotsStd[i] = 0.0;
            }
        }

        Rotation rt = getRelRot(axis);

        staticRot.print();

        //rotatePen(staticRot.angle,staticRot.axis.x,
        //staticRot.axis.y, staticRot.axis.z);

        rotatePen(rt.angle,rt.axis.x,rt.axis.y,rt.axis.z);
        rotatePen((float)rotsStd[0],1.0f,0.0f,0.0f);
        rotatePen((float)rotsStd[1],0.0f,1.0f,0.0f);
        rotatePen((float)rotsStd[2],0.0f,0.0f,1.0f);

        movePen(0,length,0);

        penBone(length);
    }

    public Rotation getRelRot(Vector3D newDir)
   {
       Vector3D a = new Vector3D(0,-1,0).unit();
       Vector3D b = newDir.unit();
       float angle = (float)Math.acos(a.dot(b));
       Vector3D axis = a.cross(b).unit();
       return new Rotation(-angle,axis);
   }

    void penBone(float length)
    {
        float square = 0.25f;
        GL11.glBegin(GL11.GL_QUADS);						// Draw A Quad

            //front
            GL11.glColor3f(1.0f,0.0f,0.0f);
            GL11.glVertex3f(-square/2.0f, 0.0f, square/2.0f);				// Top Left
            GL11.glVertex3f(square/2.0f, 0.0f, square/2.0f);				// Top Right
            GL11.glColor3f(0.0f,0.0f,1.0f);
            GL11.glVertex3f(square/2.0f,-length,square/2.0f);				// Bottom Right
            GL11.glVertex3f(-square/2.0f,-length,square/2.0f);				// Bottom Left

            //back
            GL11.glColor3f(1.0f,0.0f,0.0f);
            GL11.glVertex3f(-square/2.0f, 0.0f, -square/2.0f);				// Top Left
            GL11.glVertex3f(square/2.0f, 0.0f, -square/2.0f);				// Top Right
            GL11.glColor3f(0.0f,0.0f,1.0f);
            GL11.glVertex3f(square/2.0f,-length, -square/2.0f);				// Bottom Right
            GL11.glVertex3f(-square/2.0f,-length, -square/2.0f);				// Bottom Left

            //left
            GL11.glColor3f(1.0f,0.0f,0.0f);
            GL11.glVertex3f(-square/2.0f, 0.0f, -square/2.0f);				// Top Left
            GL11.glVertex3f(-square/2.0f, 0.0f, square/2.0f);				    // Top Right
            GL11.glColor3f(0.0f,0.0f,1.0f);
            GL11.glVertex3f(-square/2.0f,-length, square/2.0f);				// Bottom Right
            GL11.glVertex3f(-square/2.0f,-length, -square/2.0f);				// Bottom Left

            //right
            GL11.glColor3f(1.0f,0.0f,0.0f);
            GL11.glVertex3f( square/2.0f, 0.0f, square/2.0f);				// Top Left
            GL11.glVertex3f( square/2.0f, 0.0f, -square/2.0f);				// Top Right
            GL11.glColor3f(0.0f,0.0f,1.0f);
            GL11.glVertex3f( square/2.0f,-length, -square/2.0f);				// Bottom Right
            GL11.glVertex3f( square/2.0f,-length, square/2.0f);				// Bottom Left

            //top
            GL11.glColor3f(1.0f,0.0f,0.0f);
            GL11.glVertex3f(-square/2.0f, 0.0f, -square/2.0f);				// Top Left
            GL11.glVertex3f(square/2.0f, 0.0f, -square/2.0f);				// Top Right
            GL11.glVertex3f(square/2.0f, 0.0f, square/2.0f);				// Bottom Right
            GL11.glVertex3f(-square/2.0f, 0.0f, square/2.0f);				// Bottom Left

            //bottom
            GL11.glColor3f(0.0f,0.0f,1.0f);
            GL11.glVertex3f(-square/2.0f,-length,square/2.0f);				// Top Left
            GL11.glVertex3f(square/2.0f,-length,square/2.0f);				// Top Right
            GL11.glVertex3f(square/2.0f,-length, -square/2.0f);				// Bottom Right
            GL11.glVertex3f(-square/2.0f,-length, -square/2.0f);				// Bottom Left

            GL11.glEnd();
    }

    private void init() throws LWJGLException
    {
        int width = 1024;
        int height = 768;

        Display.setLocation((Display.getDisplayMode().getWidth() - 300) / 2,
												(Display.getDisplayMode().getHeight() - 300) / 2);
		Display.setDisplayMode(new DisplayMode(width, height));
		Display.setTitle("MoCrapTest");
		Display.create();

        GL11.glMatrixMode(GL11.GL_PROJECTION);
        GL11.glLoadIdentity();

	    GLU.gluPerspective(45.0f,(float)width/(float)height,0.1f,100.0f);

        GL11.glMatrixMode(GL11.GL_MODELVIEW);
        GL11.glLoadIdentity();

        GL11.glShadeModel(GL11.GL_SMOOTH);

        GL11.glClearDepth(1.0f);
        GL11.glEnable(GL11.GL_DEPTH_TEST);
        GL11.glDepthFunc(GL11.GL_LEQUAL);

        GL11.glHint(GL11.GL_PERSPECTIVE_CORRECTION_HINT,GL11.GL_NICEST);
    }
}

